﻿using Accord.Math;
using Microsoft.Kinect;
using System.Threading.Tasks;

namespace AMEE.PlaneDetection
{
    /// <summary>
    /// Some Extensions.
    /// <list type="number">
    /// <item><description>Casting Kinect 3D-Point-Array to Accord 3D-Point-Array and wise versa.</description></item>
    /// </list>
    /// </summary>
    public static class Extensions
    {
        /// <summary>
        /// Cast (Copy) a Kinect PointCloud to an Accord PointCloud.
        /// </summary>
        /// <param name="kinectPointCloud">Kinect's 3D Points.</param>
        /// <returns>Corresponding Accord 3D Points</returns>
        public static Point3[] ToAccord(this CameraSpacePoint[] kinectPointCloud) {
            if (kinectPointCloud == null) {
                return null;
            }
            Point3[] casted = new Point3[kinectPointCloud.Length];
            Parallel.For(0, kinectPointCloud.Length, idx => {
                casted[idx].X = kinectPointCloud[idx].X;
                casted[idx].Y = kinectPointCloud[idx].Y;
                casted[idx].Z = kinectPointCloud[idx].Z;
            });
            return casted;
        }

        /// <summary>
        /// Cast (Copy) a Accord PointCloud to an Kinect PointCloud.
        /// </summary>
        /// <param name="accordPointCloud">Accord's 3D Points.</param>
        /// <returns>Corresponding Kinect 3D Points</returns>
        public static CameraSpacePoint[] ToKinect(this Point3[] accordPointCloud) {
            if (accordPointCloud == null) {
                return null;
            }
            CameraSpacePoint[] casted = new CameraSpacePoint[accordPointCloud.Length];
            Parallel.For(0, accordPointCloud.Length, idx => {
                casted[idx].X = accordPointCloud[idx].X;
                casted[idx].Y = accordPointCloud[idx].Y;
                casted[idx].Y = accordPointCloud[idx].Z;
            });
            return casted;
        }
    }
}
